import os
import launch
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
from launch.substitutions import LaunchConfiguration
from webots_ros2_driver.webots_launcher import WebotsLauncher
from webots_ros2_driver.webots_controller import WebotsController


def generate_launch_description():
    '''
    StartNodes:
        - webots:
            start webots simulator
        - robot_driver:
            start external controller only apply force
    
    Tips:
        - robot_driver's topics are published in the [robot_name] namespace, if use input or core need remapping
    '''
    package_dir = get_package_share_directory("robot_sim")
    robot_description_path = os.path.join(package_dir, "webots", "direct_robot.urdf")

    webots = WebotsLauncher(world=os.path.join(package_dir, "webots", "double.wbt"))

    robot_driver1 = WebotsController(
        robot_name="robot1",
        parameters=[
            {"robot_description": robot_description_path}
        ],
        namespace="robot1",
    )
    robot_driver2 = WebotsController(
        robot_name="robot2",
        parameters=[
            {"robot_description": robot_description_path}
        ],
        namespace="robot2",
    )

    return LaunchDescription(
        [
            webots,
            robot_driver1,
            robot_driver2,
            launch.actions.RegisterEventHandler(
                event_handler=launch.event_handlers.OnProcessExit(
                    target_action=webots,
                    on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
                )
            ),
        ]
    )
